#include "body.h"

Derivative junk;

PointFP Body::acceleration(const State &state, Fixed t)
{
    const Fixed k = -10;
    const Fixed b = 1;
    return state.position*k - state.velocity*b;
}

Derivative Body::evaluate(const State &initial, Fixed t, Fixed dt=0, const Derivative &d=junk)
{
    State state;
    state.position = initial.position + (d.dPos * dt);
    state.velocity = initial.velocity + (d.dVel * dt);

    Derivative output;
    output.dPos = state.velocity;
    output.dVel = acceleration(state, t+dt);
    return output;
}

void Body::integrate(State &state, Fixed t, Fixed dt)
{
    Derivative a = evaluate( state, t);
    Derivative b = evaluate( state, t, dt*0.5f, a);
    Derivative c = evaluate( state, t, dt*0.5f, b);
    Derivative d = evaluate( state, t, dt, c);

    const PointFP dPosdt = 1/6.0f * (a.dPos + (b.dPos + c.dPos) * 2 + d.dPos);
    const PointFP dVeldt = 1/6.0f * (a.dVel + (b.dVel + c.dVel) * 2 + d.dVel);

    state.position += dPosdt * dt;
    state.velocity += dVeldt * dt;
}

void Body::update(Fixed t, Fixed dt)
{
    integrate(data, t, dt);
}
